import rclpy
from rclpy.node import Node
import serial
import threading
from geometry_msgs.msg import PoseWithCovariance,TwistWithCovarianceStamped

class GPSDriver(Node):
    '''
        GPS驱动节点
        解析NMEA字符串并发布GPS位置信息
        同时支持使用mqtt协议向GPS传输RTK数据
    '''
    def __init__(self):
        super().__init__('gps_driver')
        self.gps_pos=PoseWithCovariance()
        self.DeclareParams()
        self.InitInterfaces()
        self.threads = [threading.Thread(target=self.__UpdateGPS),]
        for thread in self.threads:
            thread.daemon = True
            thread.start()


    def DeclareParams(self):
        '''
            申明参数
        '''
        self.declare_parameter('serial_port', '/dev/ch340_dev')
        self.declare_parameter('baud_rate', 115200)
        #   mqtt-RTK数据支持， 不支持或使用硬件RTK设置为False即可
        self.declare_parameter('mqtt_rtk_support', True)    #是否支持mqtt rtk数据
        self.declare_parameter('mqtt_rtk_topic', '/rtk/beihang')   #mqtt rtk数据主题
        self.declare_parameter('mqtt_rtk_broker', 'localhost') #mqtt broker地址
        self.declare_parameter('is_mqtt_rtk_station', False) #是否是RTK基站
        #   GPS串口配置信息
        self.serial_port = self.get_parameter('serial_port').get_parameter_value().string_value
        self.baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
       
    def InitInterfaces(self):
        '''
            初始化接口
        '''
        self.pose_pub = self.create_publisher(PoseWithCovariance, 'gps_pose', 10)
        self.serial = serial.Serial(self.serial_port, self.baud_rate, timeout=None)

    def __UpdateGPS(self):
        '''
            更新GPS数据
        '''
        while rclpy.ok():
            if self.serial.in_waiting > 0:
                line = self.serial.readline().decode('utf-8').strip()
                print(line)

def main():
    rclpy.init()
    gps_driver = GPSDriver()
    try:
        rclpy.spin(gps_driver)
    except KeyboardInterrupt:
        pass
    finally:
        gps_driver.destroy_node()
        rclpy.shutdown()